# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
import inspect
from BwSensorLib import BwSensorLib
from ArmRobot import ArmRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 
from RobotKine import ArmKine

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath = os.path.dirname(os.path.abspath(fileName))
config_file = folderPath+"/config.txt"

class ArmSpatialControl:
    def __init__(self):
        # 实例化MiniBotControlLib类，并设置为udp模式
        self.mlib = MiniBotControlLib("udp")
        # 输入ip地址和端口号
        self.ip = "10.10.100.254"
        self.port = 8899
        # 连接机器人,并设置超时时间2s
        self.mlib.Connect(self.ip, self.port, 2000)
        # 实例化BwRobotLib类，并将mlib作为参数传入
        self.robotlib = BwRobotLib(self.mlib)
        # 导入配置文件，获取文件中的模块ID和模块连接位置
        self.module_ids, self.module_ids_loc = self.robotlib.loadConfig(config_file)
        # 扫描当前机器人构型的ID，默认扫描ID段为20-50
        self.module_ids_scan, self.module_ids_loc_scan = self.mlib.ScanId()
        # 判断是否扫描的ID与配置文件导入的ID一致
        res = BwHelpLib.checkListEqual(self.module_ids,self.module_ids_scan)
        if res == False:
            print("\033[31m输入的模块ID与扫描获取的模块ID不一致，请检查！\033[37m")
            print("配置文件中模块ID: ", self.module_ids)
            print("实际扫描的模块ID: ", self.module_ids_scan)
            exit(0)
        else:
            print("输入的模块ID与扫描获取的模块ID一致")
            
        # 实例化操作臂类
        self.armlib = ArmRobot(self.mlib)
        # 将模块ID与操作臂构型绑定
        self.armlib.bindIds(self.module_ids)        
        # 实例化传感器库对象
        self.sensorlib = BwSensorLib(self.mlib)
        # 机器人回到HOME位置
        self.armlib.goHome()
        # 更新当前操作臂的关节角状态
        self.armlib.updateAngles()
        # 实例化操作臂运动学类
        self.arm_kine = ArmKine()
    
    def kine_demo(self):
        armAng1_orig = [0, 30, 40, 50, 0, 0]
        armAng2_orig = [20, 30, 60, 50, 0, 0]
        armAng3_orig = [0, 30, 60, 50, 40, 0]
        armAng4_orig = [0, 30, 60, 00, 40, 0]

        arm_pos1 = self.arm_kine.fkine(armAng1_orig)
        arm_pos2 = self.arm_kine.fkine(armAng2_orig)
        arm_pos3 = self.arm_kine.fkine(armAng3_orig)
        arm_pos4 = self.arm_kine.fkine(armAng4_orig)

        armAng1 = self.arm_kine.fkine(arm_pos1)
        armAng2 = self.arm_kine.fkine(arm_pos2)
        armAng3 = self.arm_kine.fkine(arm_pos3)
        armAng4 = self.arm_kine.fkine(arm_pos4)        

        # 逆解不会计算夹持器的角度，需要自己输入，本次默认为0
        armAng1.append(0)
        armAng2.append(0)
        armAng3.append(0)
        armAng4.append(0)

        print("第一组关节角：",armAng1_orig)
        print("第一组关节角正解：",arm_pos1)
        print("第一组关节角逆解：",armAng1)
        print("第二组关节角：",armAng1_orig)
        print("第二组关节角正解：",arm_pos2)
        print("第二组关节角逆解：",armAng2)
        print("第三组关节角：",armAng3_orig)
        print("第三组关节角正解：",arm_pos3)
        print("第三组关节角逆解：",armAng3)
        print("第四组关节角：",armAng4_orig)
        print("第四组关节角正解：",arm_pos4)
        print("第四组关节角逆解：",armAng4)   


        # 真机测试
        print("\n测试第一组关节角")
        self.armlib.position_angles = armAng1
        self.armlib.pushJointsAngles(10, 4)
        time.sleep(1)
        print("\n测试第二组关节角")
        self.armlib.position_angles = armAng2
        self.armlib.pushJointsAngles(10, 4)
        time.sleep(1)
        print("\n测试第三组关节角")
        self.armlib.position_angles = armAng3
        self.armlib.pushJointsAngles(10, 4)
        time.sleep(1)
        print("\n测试第四组关节角")
        self.armlib.position_angles = armAng4
        self.armlib.pushJointsAngles(10, 4)
        time.sleep(1)

if __name__ == "__main__":
    try :
        arm = ArmSpatialControl()  
        arm.kine_demo() 
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass

